#include "Robot.h"
#include "Protocol.h"

#define MG_MAXSPEED 100

MG_t MG_Rotate;  // 旋转平台电机
MG_t MF_Lift;    // 升降电机, MF电机没有减速
MG_t MG_Camera1; // 相机电机1
MG_t MG_Camera2; // 相机电机2
MG_t MG_Camera3; // 相机电机3

/**
 * Initialize the robot
 */
inline void Robot_Init(void)
{
    AttachInterrupt_CAN(&hcan, MG_CAN_Callback);
    // AttachInterrupt_UART(&RFID_UART, 32, RFID_UART_Callback);
    // AttachInterrupt_UART(&PROTOCOL_UART, 32, Protocol_Callback);

    TSDA_Start();
    MG_Init(&MG_Rotate, 1, &hcan);
    MG_Init(&MF_Lift, 2, &hcan);
    MG_Init(&MG_Camera1, 3, &hcan);
    MG_Init(&MG_Camera2, 4, &hcan);
    MG_Init(&MG_Camera3, 5, &hcan);
}

/**
 * Set the robot speed
 * speed: mm/s
 */
inline void Robot_SetSpeed(float speed)
{
    TSDA_SetSpeed(speed / 6.0f * 22.5f);
}

/**
 * 设置旋转平台角度
 */
inline void Robot_SetRotateAngle(float angle)
{
    MG_SetAngleInc(&MG_Rotate, angle, MG_MAXSPEED);
}

/**
 * 设置伸缩杆的长度
 */
inline void Robot_SetLiftLength(float length)
{
    MG_SetAngleInc(&MF_Lift, length, MG_MAXSPEED);
}

/**
 * 设置相机1电机的角度
 */
inline void Robot_SetCamera1Angle(float angle)
{
    MG_SetAngleInc(&MG_Camera1, angle, MG_MAXSPEED);
}

/**
 * 设置相机2电机的角度
 */
inline void Robot_SetCamera2Angle(float angle)
{
    MG_SetAngleInc(&MG_Camera2, angle, MG_MAXSPEED);
}

/**
 * 设置相机3电机的角度
 */
inline void Robot_SetCamera3Angle(float angle)
{
    MG_SetAngleInc(&MG_Camera3, angle, MG_MAXSPEED);
}

inline void Robot_Run(void)
{
    Robot_SetCamera1Angle(100);
}
